#include <stdio.h>
#include <sys/time.h>
#include <math.h> 
#include "../include/uart.h"
#include <stdint.h>
#include "../include/mode.h"
#include "../include/type.h"


#define PI 3.14159265358979323846
#define FLAPPING_FREQUENCY 3 // 拍翼频率 Hz
#define FLAPPING_AMPLITUDE 40 // 拍翼摆幅 度
#define SWAYING_AMPLITUDE 70  // 摇翼摆幅 度 
#define CYCLE_TIME_MS (1000 / FLAPPING_FREQUENCY) // 单周期时间 ms
#define HALF_CYCLE_TIME_MS (CYCLE_TIME_MS / 2) // 半周期时间 ms



Motor_Ctr Speed_ClosedLoop_ctr(uint32_t speed){ //speed 单位为 dps
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = SPEED_CLOSEDLOOP_CTR;
	rs485_frame.null = 0x00;
	uint32_t speedControl = speed * 100;
	rs485_frame.data[0] = 0x00;
	rs485_frame.data[1] = 0x00;
	rs485_frame.data[2] = (uint8_t)speedControl; 
	rs485_frame.data[3] = (uint8_t)(speedControl >> 8); 
	rs485_frame.data[4] = (uint8_t)(speedControl >> 16); 
	rs485_frame.data[5] = (uint8_t)(speedControl >> 24); 
   
	return rs485_frame;
}



void Forward_Mode(void)//直游模式
//坐标系参考杨宗华师兄论文
{
    host_ctr_cmd_g.servo_ctr.servo_head_x=90;
    host_ctr_cmd_g.servo_ctr.servo_head_y=90;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_out = 90;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_out = 90;
     // 拍翼推程阶段
    for(int angle = 90; angle <= 90+FLAPPING_AMPLITUDE; angle += 5)
    {
         host_ctr_cmd_g.servo_ctr.servo_fin_left_out = angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_out = angle;
         k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
         CRC_Write();
    }
   // 拍翼回程 + 摇翼复合运动
    for(int angle = 90+FLAPPING_AMPLITUDE; angle >= 90; angle -= 5)
    {
         host_ctr_cmd_g.servo_ctr.servo_fin_left_out = angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_out = angle;

         if (angle-135>0)
         {
         int swaying_angle = SWAYING_AMPLITUDE * sin((angle-135) * PI / FLAPPING_AMPLITUDE);
         host_ctr_cmd_g.servo_ctr.servo_fin_left_in=swaying_angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_in=swaying_angle;

         k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
         CRC_Write();
         }
         else
         {
          host_ctr_cmd_g.servo_ctr.servo_fin_left_in=0;
          host_ctr_cmd_g.servo_ctr.servo_fin_right_in=0;

          k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
          CRC_Write();
         }

         
    }
}   

void Turn_Left_Mode()//左转模式
{
    host_ctr_cmd_g.servo_ctr.servo_head_x=45;
    host_ctr_cmd_g.servo_ctr.servo_head_y=90;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_out = 90;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_out = 90;
    // 拍翼推程阶段
    for(int angle=90;angle<=90+FLAPPING_AMPLITUDE;angle+=5)
    {  
        host_ctr_cmd_g.servo_ctr.servo_fin_left_out=180-angle;
        host_ctr_cmd_g.servo_ctr.servo_fin_right_out=angle;
        k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
        CRC_Write;
    }
   // 拍翼回程 + 摇翼复合运动
   for(int angle=90+FLAPPING_AMPLITUDE;angle>=90;angle-=5)
   {
       host_ctr_cmd_g.servo_ctr.servo_fin_left_out=angle;
       host_ctr_cmd_g.servo_ctr.servo_fin_right_out=180-angle;

       if (angle-135>0)
         {
         int swaying_angle = SWAYING_AMPLITUDE * sin((angle-135) * PI / FLAPPING_AMPLITUDE);
         host_ctr_cmd_g.servo_ctr.servo_fin_left_in=swaying_angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_in=swaying_angle;

         k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
         CRC_Write();
         }
         else
         {
          host_ctr_cmd_g.servo_ctr.servo_fin_left_in=0;
          host_ctr_cmd_g.servo_ctr.servo_fin_right_in=0;

          k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
          CRC_Write();
         }
   }

}

void Turn_Right_Mode()//右转模式
{
    host_ctr_cmd_g.servo_ctr.servo_head_x=135;
    host_ctr_cmd_g.servo_ctr.servo_head_y=90;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_out = 90;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_out = 90;
    // 拍翼推程阶段
    for(int angle=90;angle<=90+FLAPPING_AMPLITUDE;angle+=5)
    {  
        host_ctr_cmd_g.servo_ctr.servo_fin_left_out=angle;
        host_ctr_cmd_g.servo_ctr.servo_fin_right_out=180-angle;
        k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
        CRC_Write;
    }
   // 拍翼回程 + 摇翼复合运动
   for(int angle=90+FLAPPING_AMPLITUDE;angle>=90;angle-=5)
   {
       host_ctr_cmd_g.servo_ctr.servo_fin_left_out=180-angle;
       host_ctr_cmd_g.servo_ctr.servo_fin_right_out=angle;

       if (angle-135>0)
         {
         int swaying_angle = SWAYING_AMPLITUDE * sin((angle-135) * PI / FLAPPING_AMPLITUDE);
         host_ctr_cmd_g.servo_ctr.servo_fin_left_in=swaying_angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_in=swaying_angle;

         k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
         CRC_Write();
         }
         else
         {
          host_ctr_cmd_g.servo_ctr.servo_fin_left_in=0;
          host_ctr_cmd_g.servo_ctr.servo_fin_right_in=0;

          k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
          CRC_Write();
         }
   }

}

void Backward_Mode(void)//倒游模式
{
    host_ctr_cmd_g.servo_ctr.servo_head_x=90;
    host_ctr_cmd_g.servo_ctr.servo_head_y=90;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_in = SWAYING_AMPLITUDE;
    host_ctr_cmd_g.servo_ctr.servo_fin_left_out = 90;
    host_ctr_cmd_g.servo_ctr.servo_fin_right_out = 90;
    host_ctr_cmd_g.motor_ctr= Speed_ClosedLoop_ctr(0);
    CRC_Write();

    // 拍翼推程阶段
    for(int angle = 90; angle >= 90-FLAPPING_AMPLITUDE; angle -= 5)
    {
         host_ctr_cmd_g.servo_ctr.servo_fin_left_out = angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_out = angle;
         k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
         CRC_Write();
    }
   // 拍翼回程 + 摇翼复合运动
    for(int angle = 90-FLAPPING_AMPLITUDE; angle <= 90; angle += 5)
    {
         host_ctr_cmd_g.servo_ctr.servo_fin_left_out = angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_out = angle;

         if (angle-45<0)
         {
         int swaying_angle = SWAYING_AMPLITUDE * sin((angle-135) * PI / FLAPPING_AMPLITUDE);
         host_ctr_cmd_g.servo_ctr.servo_fin_left_in=swaying_angle;
         host_ctr_cmd_g.servo_ctr.servo_fin_right_in=swaying_angle;

         k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
         CRC_Write();
         }
         else
         {
          host_ctr_cmd_g.servo_ctr.servo_fin_left_in=0;
          host_ctr_cmd_g.servo_ctr.servo_fin_right_in=0;

          k_msleep(HALF_CYCLE_TIME_MS/(FLAPPING_AMPLITUDE/5));
          CRC_Write();
         }

         
    }
}